package com.rockwell.sniffyhunter.model;

import java.util.Map;

import com.rockwell.sniffyhunter.utils.DetectionIndex;
import com.rockwell.sniffyhunter.utils.IDetector;
import com.rockwell.sniffyhunter.utils.IRobotPositionDetector;
import com.rockwell.sniffyhunter.utils.IRobotSensors;
import com.rockwell.sniffyhunter.utils.IStateMachine;
import com.rockwell.sniffyhunter.utils.EntranceLocatorState;

/**
 * This class provides the robot to locate the maze entrance
 * The following actions will be performed:
 * <ul>
 * <li>Move along the outer walls of the maze.
 * <li>Align with the maze entrance.
 * <li>Jog until robot sense the side wall.
 * </ul>
 * <p>
 * 
 * @author      Sniffy Software Team
 * @version     1.0
 */

public class EntranceLocator extends StateObject {
	
	private static final int OVER_TRAVEL_AFTER_JOG = 125; // mm
	private static final int OVER_TRAVEL_TO_DETECT_DOOR = 110; // mm
	private static int MOVE_INDEX = 20; //mm
	private static int TURN_INDEX = 10; //degree
	private static int MAX_TURN_INDEX = 5;
	private int m_turn_count;
	private int m_turn_direction = 1; // 1: Clockwise
	private int m_frontWallHitRecoverDirection = 1; // 1: Clockwise
	private RobotCommandSequencer m_commandSequencer;
	private RobotCommand m_command;
	private RobotPosition m_robotPosition;
	private IRobotSensors m_sensors;
	private IRobotPositionDetector m_positionDetector;
	private Map<Integer, IDetector> m_colorDetectors;
	private Picker m_picker;
	private int m_maze;

	private ExitCode m_exitCode;
	private enum ExitCode {
		NOT_STARTED,
		ENTRANCE_FOUND,
		NO_ENTRANCE_FOUND
	}

	public EntranceLocator(Robot robot) {
		super(robot);
		m_commandSequencer = new RobotCommandSequencer(robot);
		m_command = new RobotCommand();
		m_positionDetector = robot.m_positionDetector;
		m_sensors = robot.m_sensors;
		m_colorDetectors = robot.m_colorDetectors;
		m_picker = new Picker(robot);
	}
	
	public void setMaze(int maze) {
		m_maze = maze;
	}
	
	@Override
	public void Initialize() {
		//m_commandSequencer.Initialize();
		//m_picker.Initialize();
		m_turn_count = 0;
		machineState = EntranceLocatorState.START;
		machineRecoveryState = machineState;
		m_exitCode = ExitCode.NOT_STARTED;
	}
	
	@Override
	public boolean IsDone() {
		return machineState == EntranceLocatorState.DONE;
	}

	public IStateMachine start() {

		m_robotPosition = m_positionDetector.getPosition();

		if (m_robotPosition.leftwalldetected) {
			m_turn_direction = -1; // turn left
		}
		else if (m_robotPosition.rightwalldetected) {
			m_turn_direction = 1; // turn right
		}
		else {
			assert(false);
			m_turn_direction = -1; // turn left
		}
		
		m_exitCode = ExitCode.NOT_STARTED;
		machineState = EntranceLocatorState.DETECT_NEXT_MAZE_WALL;
		return machineState;
	}
	
	public IStateMachine jogAlongMazeWall() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.Jog, (m_turn_direction == -1? 2 : 6));

		if (returnVal == ReturnResult.Completed) {
			log("jogAlongMazeWall.EntranceLocator", 4);
			m_turn_count = 0;
			machineState = EntranceLocatorState.SCAN;
		}
		
		return machineState;
	}
	
	public IStateMachine recoverJogAlongMazeWall() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.TurnDegree, m_turn_direction == 1? -TURN_INDEX : TURN_INDEX);

		if (returnVal == ReturnResult.Completed) {
			log("jogAlongMazeWall.EntranceLocator", 4);
			machineState = EntranceLocatorState.SCAN;
		}
		
		return machineState;
	}
	
	public IStateMachine scan() {

		m_robotPosition = m_positionDetector.getPosition();
		
		if (m_colorDetectors.get(DetectionIndex.TREASURE).isPresent()) {
			machineRecoveryState = EntranceLocatorState.PICK_TREASURE;
			machineState = EntranceLocatorState.STOP_JOG;
		}
		else if (!m_sensors.isWheelMoving()) {
			if (m_robotPosition.frontwalldetected) {
				machineState = EntranceLocatorState.RECOVER_JOG_ALONG_MAZE_WALL;
			}
			else if (!m_robotPosition.leftwalldetected && !m_robotPosition.rightwalldetected) { // Corner found!
				machineState = EntranceLocatorState.OVER_JOG_OUT_OF_MAZE_WALL;
				
				if (m_colorDetectors.get(DetectionIndex.MAZE_EXIT).isPresent()) {
					m_exitCode = ExitCode.ENTRANCE_FOUND;
				}
			}
			else {
				machineState = EntranceLocatorState.JOG_ALONG_MAZE_WALL;
			}
		}
		else if (m_turn_direction == -1 && m_robotPosition.isTooLeft()) {
			machineRecoveryState = EntranceLocatorState.RECOVER_JOG_ALONG_MAZE_WALL;
			machineState = EntranceLocatorState.STOP_JOG;
		}
		else if (m_turn_direction == 1 && m_robotPosition.isTooRight()) {
			machineRecoveryState = EntranceLocatorState.RECOVER_JOG_ALONG_MAZE_WALL;
			machineState = EntranceLocatorState.STOP_JOG;
		}

		return machineState;
	}
	
	public IStateMachine stopJog() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.Stop);

		if (returnVal == ReturnResult.Completed) {
			log("stopJog.EntranceLocator", 4);
			machineState = machineRecoveryState;
		}
		
		return machineState;
	}
	
	public IStateMachine overtravelOutOfMazeWall() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.Move2, OVER_TRAVEL_AFTER_JOG);

		if (returnVal == ReturnResult.Completed) {

			log("overtravelOutOfMazeWall.EntranceLocator", 4);
			machineState = EntranceLocatorState.TURN;
		}
		
		return machineState;
	}
	
	public IStateMachine overJogOutOfMazeWall() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.Jog, (m_turn_direction == -1? 9 : 10));

		if (returnVal == ReturnResult.Completed) {
			log("overJogOutOfMazeWall.EntranceLocator", 4);
			sleep(1000);
			machineState = EntranceLocatorState.CHECK_OVER_JOG_OUT_OF_MAZE_WALL_DONE;
		}
		
		return machineState;
	}
	
	public IStateMachine checkOverJogOutOfMazeWallDone() {

		if (!m_sensors.isWheelMoving()) {
			log("checkOverJogOutOfMazeWallDone.EntranceLocator", 4);
			machineState = EntranceLocatorState.TURN;
		}

		return machineState;
	}
	
	public IStateMachine turn() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.TurnDegree, m_turn_direction == 1? 85 : -85);

		if (returnVal == ReturnResult.Completed) {
			log("turn.EntranceLocator", 4);
			
			if (m_colorDetectors.get(DetectionIndex.MAZE_EXIT).isPresent()) {
				m_exitCode = ExitCode.ENTRANCE_FOUND;
			}
			
			machineState = EntranceLocatorState.CHECK_OVERTURN_DONE;
		}
		
		return machineState;
	}
	
	public IStateMachine overturn() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.TurnDegree, m_turn_direction == 1? TURN_INDEX : -TURN_INDEX);

		if (returnVal == ReturnResult.Completed) {
			log("overturn.EntranceLocator", 4);
			
			if (m_colorDetectors.get(DetectionIndex.MAZE_EXIT).isPresent()) {
				m_exitCode = ExitCode.ENTRANCE_FOUND;
			}

			machineState = EntranceLocatorState.CHECK_OVERTURN_DONE;
		}
		
		return machineState;
	}
	
	public IStateMachine checkOverturnDone() {

		sleep(1000);
		if (m_colorDetectors.get(m_maze).isPresent()) {
			machineState = EntranceLocatorState.TURN_BACK_OVERTRAVEL_AFTER_TURN;
		}
		else {
			machineState = EntranceLocatorState.OVERTURN;
		}
		
		return machineState;
	}
	
	public IStateMachine turnBackAfterCheckOverturn() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.TurnDegree, m_turn_direction == 1? -(TURN_INDEX * 0.5) : (TURN_INDEX * 0.5));

		if (returnVal == ReturnResult.Completed) {
			log("overturn.EntranceLocator", 4);

			machineState = EntranceLocatorState.OVERTRAVEL_AFTER_TURN;
		}
		
		return machineState;
	}
	
	public IStateMachine checkturnBackAfterCheckOverturnDone() {

		sleep(1000);
		if (m_colorDetectors.get(m_maze).isPresent()) {
			machineState = EntranceLocatorState.TURN_BACK_OVERTRAVEL_AFTER_TURN;
		}
		else {
			machineState = EntranceLocatorState.OVERTRAVEL_AFTER_TURN;
		}
		
		return machineState;
	}

	public IStateMachine overTravelAfterTurn() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.Move2, OVER_TRAVEL_TO_DETECT_DOOR);

		if (returnVal == ReturnResult.Completed) {
			log("overTravelAfterTurn.EntranceLocator", 4);
			
			m_robotPosition = m_positionDetector.getPosition();
			
			if (m_robotPosition.leftwalldetected && m_robotPosition.rightwalldetected)
				m_exitCode = ExitCode.ENTRANCE_FOUND;
					
			if (m_exitCode == ExitCode.ENTRANCE_FOUND) {
				log("entrance found by camera!.EntranceLocator", 4);
				machineState = EntranceLocatorState.DONE;
			}
			else {
				machineState = EntranceLocatorState.DETECT_NEXT_MAZE_WALL;
			}
		}
		
		return machineState;
	}
	
	public IStateMachine detectNextMazeWall() {

		int values[];
		
		if (m_turn_direction == 1) {
			values = m_sensors.getRightDistances();
		}
		else {
			values = m_sensors.getLeftDistances();
		}
		
		if (values == null) {
			return machineState;
		}
		
		m_robotPosition = m_positionDetector.getPosition();
		
		if (m_robotPosition.frontwalldetected) {
			if (m_turn_direction == 1) {
				if (m_exitCode == ExitCode.ENTRANCE_FOUND) {
					m_frontWallHitRecoverDirection = 1;
				}
				else {
					m_frontWallHitRecoverDirection = -1;
				}
			}
			else {
				if (m_exitCode == ExitCode.ENTRANCE_FOUND) {
					m_frontWallHitRecoverDirection = -1;
				}
				else {
					m_frontWallHitRecoverDirection = 1;
				}
			}
			machineState = EntranceLocatorState.RECOVER_FRONT_WALL_HIT;
		}
		else if (values[0] < 4500 && m_turn_count < MAX_TURN_INDEX) {
			machineState = EntranceLocatorState.OVERTURN_TO_LOCATE_MAZE_WALL;
		}
		else {
			m_turn_count = 0;
			
			
			if (m_exitCode == ExitCode.ENTRANCE_FOUND) {
				log("entrance found by camera!.EntranceLocator", 4);
				machineState = EntranceLocatorState.DONE;
			}
			else {
				log("continue locating entrance.EntranceLocator", 4);
				machineState = EntranceLocatorState.JOG_ALONG_MAZE_WALL;
			}
		}
		
		return machineState;
	}
	
	public IStateMachine recoverFrontWallHit() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.TurnDegree, (m_frontWallHitRecoverDirection == 1? TURN_INDEX : -TURN_INDEX));

		if (returnVal == ReturnResult.Completed) {
			log("recoverFrontWalHit.EntranceLocator", 4);
			machineState = EntranceLocatorState.OVERTRAVEL_AFTER_TURN;
		}
		
		return machineState;
	}
	
	public IStateMachine overturnToLocateMazeWall() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.TurnDegree, (m_turn_direction == 1? (TURN_INDEX * 2) : -(TURN_INDEX * 2)));

		if (returnVal == ReturnResult.Completed) {
			log("overturnToLocateMazeWall.EntranceLocator", 4);
			++m_turn_count;
			machineState = EntranceLocatorState.CHECK_OVERTURN_TO_LOCATE_MAZE_WALL_DONE;
		}
		
		return machineState;
	}
	
	public IStateMachine checkOverturnToLocateMazeWallDone() {
		int values[];
		
		if (m_turn_direction == 1) {
			values = m_sensors.getRightDistances();
		}
		else {
			values = m_sensors.getLeftDistances();
		}
		
		if (values[0] < 5000) {
			if (m_turn_count >= MAX_TURN_INDEX) {
				machineState = EntranceLocatorState.DETECT_NEXT_MAZE_WALL;
			}
			else {
				machineState = EntranceLocatorState.OVERTURN_TO_LOCATE_MAZE_WALL;
			}
		}
		else {
			machineState = EntranceLocatorState.DETECT_NEXT_MAZE_WALL;
		}
		
		return machineState;
	}
	
	public IStateMachine pickTreasure() {

		ReturnResult returnVal = m_picker.PerformAction();

		if (returnVal == ReturnResult.Completed) {
			log("pickTreasure.EntranceLocator", 4);
			machineState = EntranceLocatorState.SCAN;
		}
		
		return machineState;
	}
}